//
//  EggController.cpp
//  symbrion
//
//  Created by Berend Weel on 3/22/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#include <cmath>

#include "constants.h"
#include "EggController.h"

EggController::EggController(WorldModel *worldModel, RobotType *robot)  : Controller (worldModel, robot){
    state = LISTENING;
    stateChanged = true;
    lastTime = worldModel->time;
    
    organism = NULL;
}

void EggController::step() {
    for(int i=0;i<worldModel->irMsgReceived.size();i++){
        std::string msg((char*)worldModel->irMsgReceived[i].getData());
        if(msg.substr(0,4) == "org:"){
            if(organism == NULL){
                sequence = new Shape::Sequence(worldModel->irMsgReceived[i].getData(),(int)worldModel->irMsgReceived[i].getDataLength());
                organism = sequence->Deserialize();
                
                std::cout << "We have an organism sequence: " << std::endl << *sequence << std::endl;
                
                organism->MutateFixed();
                std::cout << "We have an organism shape: " << std::endl << *organism << std::endl;
                state = RECRUITING;
                stateChanged = true;
            }
        }
    }
    
    if(stateChanged){
        switch (state){
            case LISTENING:
                for(int i=0;i<robot->getNumLEDs();i++){
                    worldModel->ledColours.insert(std::pair<int,int>(i,MAGENTA));
                }
                break;
            case RECRUITING:
                setRecruitLEDs();
                break;
        }
        stateChanged = false;
    }
}

void EggController::setRecruitLEDs(){
    using namespace Shape;
    for (side_t s = 0; s < SIDE_COUNT; s++)
	{
        Node *child = organism->GetRoot()->GetConnection(s);
	    if (child != NULL) {
            for(int i=s*4;i<s*4+4;i++){
                worldModel->ledColours.insert(std::pair<int,int>(i,ORANGE));
            }
	    }
	}
}